"""
飞行历史记录

date: 2025/8/16
author: SiHeng Tang
file: history.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
import logging
import time
from copy import deepcopy

from async_sche import TaskWrapper, BaseTaskGen, ExitGenerationError
from helpers.history_interpolation import ALL_INTERPOLATIONS, DistantHistoryError
from mengji import Vehicle
from mj_errors import MJRuntimeError
from mod_loader import ConfigHandler
from task_element import DataEndpoint, DataPack

logger = logging.getLogger(__name__)
handler = ConfigHandler(__name__)

DEFAULT_CONFIG = {
    "label": "history",
    "interpolation_mtd": "liner",
    "mode": "smooth",
    "trace_time_sec": 60.0,
    "smooth_interval": 0.5
}

AVAILABLE_MODE = ["smooth", "memory"]


class HistoryProcessor(BaseTaskGen):
    """
    飞行历史记录提供飞行轨迹回溯，使用指定的方法进行采样帧之间的插值，实现飞行数据后处理。
    此节点提供两种模式::

        1. 指定一段历史长度，保持历史长度并提供这段时间内历史记录的随机访问。
        2. 指定一段时间间隔，此节点将平滑不均匀的数据包流，将其转换为投放寻径需要的均匀时间。

    此节点使用的时间是数据包的 pack_time 元数据。如果是飞行遥测的数据源，为系统时间。
    此节点设计为一种良好支持多实例的节点，如果多个模块需要缓冲数据模式，可以创建多个节点。
    """

    def __init__(self, config):
        super().__init__(priority=TaskWrapper.IDLE_PRIORITY + 10)
        self._config = config
        self._interpolation = ALL_INTERPOLATIONS[self._config["interpolation_mtd"]](self._config["trace_time_sec"])

        if self._config["interpolation_mtd"] not in ALL_INTERPOLATIONS.keys():
            raise MJRuntimeError(MJRuntimeError.PROCESSOR_ERROR, "Invalid interpolation implement")

        if self._config["mode"] not in AVAILABLE_MODE:
            raise MJRuntimeError(MJRuntimeError.PROCESSOR_ERROR, "Invalid history mode selection")

        # 平滑模式外加发布端点
        if self._config["mode"] == "smooth":
            self.pub_history_ep = DataEndpoint()
        self.recv_gps_ep = DataEndpoint()

        self._last_pub_history_time = 0
        self._last_update_time = 0

    def _on_gen(self):
        # 几种可能：平滑模式和储存模式，GPS 输入更新和达到间隔时间
        if self._config["mode"] == "smooth":
            # 有 GPS 更新或者达到预测间隔时执行处理函数
            if not (self.recv_gps_ep.detect_new_in_dp() or
                    (time.time() - self._last_pub_history_time) > self._config["smooth_interval"]):
                raise ExitGenerationError()
        else:
            # 储存模式只管 GPS 更新
            if not self.recv_gps_ep.detect_new_in_dp():
                raise ExitGenerationError()

    async def _func(self):
        self._last_update_time = (dp := self.recv_gps_ep.get_dp()).get_a("pack_time")
        self._interpolation.update_frame(dp)

        if self._config["mode"] != "smooth":
            return

        # 通过最后一次输出帧时间推算下一帧绝对时间，实现高精度满足等长数据间隔
        if (tat := self._last_pub_history_time + self._config["smooth_interval"]) < self._last_update_time:
            try:
                odp = self._interpolation.get_history_at(self._last_update_time - tat)
            except DistantHistoryError:
                logger.critical(f"Distant history, is system over loaded?")
                self._last_pub_history_time = time.time()
            else:
                self.pub_history_ep.set_dp(odp)
                self._last_pub_history_time = odp.get_a("pack_time")

    def _on_exit(self):
        self.recv_gps_ep = None
        self._interpolation = None

        if self._config["mode"] == "smooth":
            self.pub_history_ep = None

    #########################
    # 历史记录访问
    #########################
    def get_history_at(self, trace_sec: float) -> DataPack:
        """
        获取指定时间的历史记录，超时产生运行时异常
        :param trace_sec: 回溯时间，不大于配置文件 trace_time_sec
        :return: 飞行器遥测 GPS 数据包
        """
        return self._interpolation.get_history_at(trace_sec)


@handler.add_handler("virtual_bus:history")
def load_config(veh: Vehicle, cfg_section: dict):
    logger.debug(f"history load config {cfg_section}")

    cfg = deepcopy(DEFAULT_CONFIG)
    cfg.update(cfg_section)

    veh.reg_part(cfg["label"], HistoryProcessor(cfg), is_sche=True)
